// David Eberly, Geometric Tools, Redmond WA 98052
// Copyright (c) 1998-2020
// Distributed under the Boost Software License, Version 1.0.
// https://www.boost.org/LICENSE_1_0.txt
// https://www.geometrictools.com/License/Boost/LICENSE_1_0.txt
// Version: 4.0.2019.08.13

#pragma once

#include <Mathematics/AxisAngle.h>
#include <Mathematics/EulerAngles.h>
#include <Mathematics/Matrix.h>
#include <Mathematics/Quaternion.h>

namespace gte {
// Conversions among various representations of rotations.  The value of
// N must be 3 or 4.  The latter case supports affine algebra when you use
// 4-tuple vectors (w-component is 1 for points and 0 for vector) and 4x4
// matrices for affine transformations.  Rotation axes must be unit
// length.  The angles are in radians.  The Euler angles are in world
// coordinates; we have not yet added support for body coordinates.

template <int N, typename Real> class Rotation {
public:
  // Create rotations from various representations.
  Rotation(Matrix<N, N, Real> const &matrix)
      : mType(IS_MATRIX), mMatrix(matrix) {
    static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");
  }

  Rotation(Quaternion<Real> const &quaternion)
      : mType(IS_QUATERNION), mQuaternion(quaternion) {
    static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");
  }

  Rotation(AxisAngle<N, Real> const &axisAngle)
      : mType(IS_AXIS_ANGLE), mAxisAngle(axisAngle) {
    static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");
  }

  Rotation(EulerAngles<Real> const &eulerAngles)
      : mType(IS_EULER_ANGLES), mEulerAngles(eulerAngles) {
    static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");
  }

  // Convert one representation to another.
  operator Matrix<N, N, Real>() const {
    static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");

    switch (mType) {
    case IS_MATRIX:
      break;
    case IS_QUATERNION:
      Convert(mQuaternion, mMatrix);
      break;
    case IS_AXIS_ANGLE:
      Convert(mAxisAngle, mMatrix);
      break;
    case IS_EULER_ANGLES:
      Convert(mEulerAngles, mMatrix);
      break;
    }

    return mMatrix;
  }

  operator Quaternion<Real>() const {
    static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");

    switch (mType) {
    case IS_MATRIX:
      Convert(mMatrix, mQuaternion);
      break;
    case IS_QUATERNION:
      break;
    case IS_AXIS_ANGLE:
      Convert(mAxisAngle, mQuaternion);
      break;
    case IS_EULER_ANGLES:
      Convert(mEulerAngles, mQuaternion);
      break;
    }

    return mQuaternion;
  }

  operator AxisAngle<N, Real>() const {
    static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");

    switch (mType) {
    case IS_MATRIX:
      Convert(mMatrix, mAxisAngle);
      break;
    case IS_QUATERNION:
      Convert(mQuaternion, mAxisAngle);
      break;
    case IS_AXIS_ANGLE:
      break;
    case IS_EULER_ANGLES:
      Convert(mEulerAngles, mAxisAngle);
      break;
    }

    return mAxisAngle;
  }

  EulerAngles<Real> const &operator()(int i0, int i1, int i2) const {
    static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");

    mEulerAngles.axis[0] = i0;
    mEulerAngles.axis[1] = i1;
    mEulerAngles.axis[2] = i2;

    switch (mType) {
    case IS_MATRIX:
      Convert(mMatrix, mEulerAngles);
      break;
    case IS_QUATERNION:
      Convert(mQuaternion, mEulerAngles);
      break;
    case IS_AXIS_ANGLE:
      Convert(mAxisAngle, mEulerAngles);
      break;
    case IS_EULER_ANGLES:
      break;
    }

    return mEulerAngles;
  }

private:
  enum RepresentationType {
    IS_MATRIX,
    IS_QUATERNION,
    IS_AXIS_ANGLE,
    IS_EULER_ANGLES
  };

  RepresentationType mType;
  mutable Matrix<N, N, Real> mMatrix;
  mutable Quaternion<Real> mQuaternion;
  mutable AxisAngle<N, Real> mAxisAngle;
  mutable EulerAngles<Real> mEulerAngles;

  // Convert a rotation matrix to a quaternion.
  //
  // x^2 = (+r00 - r11 - r22 + 1)/4
  // y^2 = (-r00 + r11 - r22 + 1)/4
  // z^2 = (-r00 - r11 + r22 + 1)/4
  // w^2 = (+r00 + r11 + r22 + 1)/4
  // x^2 + y^2 = (1 - r22)/2
  // z^2 + w^2 = (1 + r22)/2
  // y^2 - x^2 = (r11 - r00)/2
  // w^2 - z^2 = (r11 + r00)/2
  // x*y = (r01 + r10)/4
  // x*z = (r02 + r20)/4
  // y*z = (r12 + r21)/4
  // [GTE_USE_MAT_VEC]
  //   x*w = (r21 - r12)/4
  //   y*w = (r02 - r20)/4
  //   z*w = (r10 - r01)/4
  // [GTE_USE_VEC_MAT]
  //   x*w = (r12 - r21)/4
  //   y*w = (r20 - r02)/4
  //   z*w = (r01 - r10)/4
  //
  // If Q is the 4x1 column vector (x,y,z,w), the previous equations
  // give us
  //         +-                  -+
  //         | x*x  x*y  x*z  x*w |
  // Q*Q^T = | y*x  y*y  y*z  y*w |
  //         | z*x  z*y  z*z  z*w |
  //         | w*x  w*y  w*z  w*w |
  //         +-                  -+
  // The code extracts the row of maximum length, normalizing it to
  // obtain the result q.
  static void Convert(Matrix<N, N, Real> const &r, Quaternion<Real> &q) {
    static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");

    Real r22 = r(2, 2);
    if (r22 <= (Real)0) // x^2 + y^2 >= z^2 + w^2
    {
      Real dif10 = r(1, 1) - r(0, 0);
      Real omr22 = (Real)1 - r22;
      if (dif10 <= (Real)0) // x^2 >= y^2
      {
        Real fourXSqr = omr22 - dif10;
        Real inv4x = ((Real)0.5) / std::sqrt(fourXSqr);
        q[0] = fourXSqr * inv4x;
        q[1] = (r(0, 1) + r(1, 0)) * inv4x;
        q[2] = (r(0, 2) + r(2, 0)) * inv4x;
#if defined(GTE_USE_MAT_VEC)
        q[3] = (r(2, 1) - r(1, 2)) * inv4x;
#else
        q[3] = (r(1, 2) - r(2, 1)) * inv4x;
#endif
      } else // y^2 >= x^2
      {
        Real fourYSqr = omr22 + dif10;
        Real inv4y = ((Real)0.5) / std::sqrt(fourYSqr);
        q[0] = (r(0, 1) + r(1, 0)) * inv4y;
        q[1] = fourYSqr * inv4y;
        q[2] = (r(1, 2) + r(2, 1)) * inv4y;
#if defined(GTE_USE_MAT_VEC)
        q[3] = (r(0, 2) - r(2, 0)) * inv4y;
#else
        q[3] = (r(2, 0) - r(0, 2)) * inv4y;
#endif
      }
    } else // z^2 + w^2 >= x^2 + y^2
    {
      Real sum10 = r(1, 1) + r(0, 0);
      Real opr22 = (Real)1 + r22;
      if (sum10 <= (Real)0) // z^2 >= w^2
      {
        Real fourZSqr = opr22 - sum10;
        Real inv4z = ((Real)0.5) / std::sqrt(fourZSqr);
        q[0] = (r(0, 2) + r(2, 0)) * inv4z;
        q[1] = (r(1, 2) + r(2, 1)) * inv4z;
        q[2] = fourZSqr * inv4z;
#if defined(GTE_USE_MAT_VEC)
        q[3] = (r(1, 0) - r(0, 1)) * inv4z;
#else
        q[3] = (r(0, 1) - r(1, 0)) * inv4z;
#endif
      } else // w^2 >= z^2
      {
        Real fourWSqr = opr22 + sum10;
        Real inv4w = ((Real)0.5) / std::sqrt(fourWSqr);
#if defined(GTE_USE_MAT_VEC)
        q[0] = (r(2, 1) - r(1, 2)) * inv4w;
        q[1] = (r(0, 2) - r(2, 0)) * inv4w;
        q[2] = (r(1, 0) - r(0, 1)) * inv4w;
#else
        q[0] = (r(1, 2) - r(2, 1)) * inv4w;
        q[1] = (r(2, 0) - r(0, 2)) * inv4w;
        q[2] = (r(0, 1) - r(1, 0)) * inv4w;
#endif
        q[3] = fourWSqr * inv4w;
      }
    }
  }

  // Convert a quaterion q = x*i + y*j + z*k + w to a rotation matrix.
  // [GTE_USE_MAT_VEC]
  //     +-           -+   +-                                     -+
  // R = | r00 r01 r02 | = | 1-2y^2-2z^2  2(xy-zw)     2(xz+yw)    |
  //     | r10 r11 r12 |   | 2(xy+zw)     1-2x^2-2z^2  2(yz-xw)    |
  //     | r20 r21 r22 |   | 2(xz-yw)     2(yz+xw)     1-2x^2-2y^2 |
  //     +-           -+   +-                                     -+
  // [GTE_USE_VEC_MAT]
  //     +-           -+   +-                                     -+
  // R = | r00 r01 r02 | = | 1-2y^2-2z^2  2(xy+zw)     2(xz-yw)    |
  //     | r10 r11 r12 |   | 2(xy-zw)     1-2x^2-2z^2  2(yz+xw)    |
  //     | r20 r21 r22 |   | 2(xz+yw)     2(yz-xw)     1-2x^2-2y^2 |
  //     +-           -+   +-                                     -+
  static void Convert(Quaternion<Real> const &q, Matrix<N, N, Real> &r) {
    static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");

    r.MakeIdentity();

    Real twoX = ((Real)2) * q[0];
    Real twoY = ((Real)2) * q[1];
    Real twoZ = ((Real)2) * q[2];
    Real twoXX = twoX * q[0];
    Real twoXY = twoX * q[1];
    Real twoXZ = twoX * q[2];
    Real twoXW = twoX * q[3];
    Real twoYY = twoY * q[1];
    Real twoYZ = twoY * q[2];
    Real twoYW = twoY * q[3];
    Real twoZZ = twoZ * q[2];
    Real twoZW = twoZ * q[3];

#if defined(GTE_USE_MAT_VEC)
    r(0, 0) = (Real)1 - twoYY - twoZZ;
    r(0, 1) = twoXY - twoZW;
    r(0, 2) = twoXZ + twoYW;
    r(1, 0) = twoXY + twoZW;
    r(1, 1) = (Real)1 - twoXX - twoZZ;
    r(1, 2) = twoYZ - twoXW;
    r(2, 0) = twoXZ - twoYW;
    r(2, 1) = twoYZ + twoXW;
    r(2, 2) = (Real)1 - twoXX - twoYY;
#else
    r(0, 0) = (Real)1 - twoYY - twoZZ;
    r(1, 0) = twoXY - twoZW;
    r(2, 0) = twoXZ + twoYW;
    r(0, 1) = twoXY + twoZW;
    r(1, 1) = (Real)1 - twoXX - twoZZ;
    r(2, 1) = twoYZ - twoXW;
    r(0, 2) = twoXZ - twoYW;
    r(1, 2) = twoYZ + twoXW;
    r(2, 2) = (Real)1 - twoXX - twoYY;
#endif
  }

  // Convert a rotation matrix to an axis-angle pair.  Let (x0,x1,x2) be
  // the axis let t be an angle of rotation.  The rotation matrix is
  // [GTE_USE_MAT_VEC]
  //   R = I + sin(t)*S + (1-cos(t))*S^2
  // or
  // [GTE_USE_VEC_MAT]
  //   R = I - sin(t)*S + (1-cos(t))*S^2
  // where I is the identity and S = {{0,-x2,x1},{x2,0,-x0},{-x1,x0,0}}
  // where the inner-brace triples are the rows of the matrix.  If
  // t > 0, R represents a counterclockwise rotation; see the comments
  // for the constructor Matrix3x3(axis,angle).  It may be shown that
  // cos(t) = (trace(R)-1)/2 and R - Transpose(R) = 2*sin(t)*S.  As long
  // as sin(t) is not zero, we may solve for S in the second equation,
  // which produces the axis direction U = (S21,S02,S10).  When t = 0,
  // the rotation is the identity, in which case any axis direction is
  // valid; we choose (1,0,0).  When t = pi, it must be that
  // R - Transpose(R) = 0, which prevents us from extracting the axis.
  // Instead, note that (R+I)/2 = I+S^2 = U*U^T, where U is a
  // unit-length axis direction.
  static void Convert(Matrix<N, N, Real> const &r, AxisAngle<N, Real> &a) {
    static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");

    Real trace = r(0, 0) + r(1, 1) + r(2, 2);
    Real half = (Real)0.5;
    Real cs = half * (trace - (Real)1);
    cs = std::max(std::min(cs, (Real)1), (Real)-1);
    a.angle = std::acos(cs); // The angle is in [0,pi].
    a.axis.MakeZero();

    if (a.angle > (Real)0) {
      if (a.angle < (Real)GTE_C_PI) {
        // The angle is in (0,pi).
#if defined(GTE_USE_MAT_VEC)
        a.axis[0] = r(2, 1) - r(1, 2);
        a.axis[1] = r(0, 2) - r(2, 0);
        a.axis[2] = r(1, 0) - r(0, 1);
        Normalize(a.axis);
#else
        a.axis[0] = r(1, 2) - r(2, 1);
        a.axis[1] = r(2, 0) - r(0, 2);
        a.axis[2] = r(0, 1) - r(1, 0);
        Normalize(a.axis);
#endif
      } else {
        // The angle is pi, in which case R is symmetric and
        // R+I = 2*(I+S^2) = 2*U*U^T, where U = (u0,u1,u2) is the
        // unit-length direction of the rotation axis.  Determine
        // the largest diagonal entry of R+I and normalize the
        // corresponding row to produce U.  It does not matter the
        // sign on u[d] for chosen diagonal d, because
        // R(U,pi) = R(-U,pi).
        Real one = (Real)1;
        if (r(0, 0) >= r(1, 1)) {
          if (r(0, 0) >= r(2, 2)) {
            // r00 is maximum diagonal term
            a.axis[0] = r(0, 0) + one;
            a.axis[1] = half * (r(0, 1) + r(1, 0));
            a.axis[2] = half * (r(0, 2) + r(2, 0));
          } else {
            // r22 is maximum diagonal term
            a.axis[0] = half * (r(2, 0) + r(0, 2));
            a.axis[1] = half * (r(2, 1) + r(1, 2));
            a.axis[2] = r(2, 2) + one;
          }
        } else {
          if (r(1, 1) >= r(2, 2)) {
            // r11 is maximum diagonal term
            a.axis[0] = half * (r(1, 0) + r(0, 1));
            a.axis[1] = r(1, 1) + one;
            a.axis[2] = half * (r(1, 2) + r(2, 1));
          } else {
            // r22 is maximum diagonal term
            a.axis[0] = half * (r(2, 0) + r(0, 2));
            a.axis[1] = half * (r(2, 1) + r(1, 2));
            a.axis[2] = r(2, 2) + one;
          }
        }
        Normalize(a.axis);
      }
    } else {
      // The angle is 0 and the matrix is the identity.  Any axis
      // will work, so choose the Unit(0) axis.
      a.axis[0] = (Real)1;
    }
  }

  // Convert an axis-angle pair to a rotation matrix.  Assuming
  // (x0,x1,x2) is for a right-handed world (x0 to right, x1 up, x2 out
  // of plane of page), a positive angle corresponds to a
  // counterclockwise rotation from the perspective of an observer
  // looking at the origin of the plane of rotation and having view
  // direction the negative of the axis direction.  The coordinate-axis
  // rotations are the following, where unit(0) = (1,0,0),
  // unit(1) = (0,1,0), unit(2) = (0,0,1),
  // [GTE_USE_MAT_VEC]
  //   R(unit(0),t) = {{ 1, 0, 0}, { 0, c,-s}, { 0, s, c}}
  //   R(unit(1),t) = {{ c, 0, s}, { 0, 1, 0}, {-s, 0, c}}
  //   R(unit(2),t) = {{ c,-s, 0}, { s, c, 0}, { 0, 0, 1}}
  // or
  // [GTE_USE_VEC_MAT]
  //   R(unit(0),t) = {{ 1, 0, 0}, { 0, c, s}, { 0,-s, c}}
  //   R(unit(1),t) = {{ c, 0,-s}, { 0, 1, 0}, { s, 0, c}}
  //   R(unit(2),t) = {{ c, s, 0}, {-s, c, 0}, { 0, 0, 1}}
  // where c = cos(t), s = sin(t), and the inner-brace triples are rows
  // of the matrix.  The general matrix is
  // [GTE_USE_MAT_VEC]
  //      +-                                                          -+
  //  R = | (1-c)*x0^2  + c     (1-c)*x0*x1 - s*x2  (1-c)*x0*x2 + s*x1 |
  //      | (1-c)*x0*x1 + s*x2  (1-c)*x1^2  + c     (1-c)*x1*x2 - s*x0 |
  //      | (1-c)*x0*x2 - s*x1  (1-c)*x1*x2 + s*x0  (1-c)*x2^2  + c    |
  //      +-                                                          -+
  // [GTE_USE_VEC_MAT]
  //      +-                                                          -+
  //  R = | (1-c)*x0^2  + c     (1-c)*x0*x1 + s*x2  (1-c)*x0*x2 - s*x1 |
  //      | (1-c)*x0*x1 - s*x2  (1-c)*x1^2  + c     (1-c)*x1*x2 + s*x0 |
  //      | (1-c)*x0*x2 + s*x1  (1-c)*x1*x2 - s*x0  (1-c)*x2^2  + c    |
  //      +-                                                          -+
  static void Convert(AxisAngle<N, Real> const &a, Matrix<N, N, Real> &r) {
    static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");

    r.MakeIdentity();

    Real cs = std::cos(a.angle);
    Real sn = std::sin(a.angle);
    Real oneMinusCos = ((Real)1) - cs;
    Real x0sqr = a.axis[0] * a.axis[0];
    Real x1sqr = a.axis[1] * a.axis[1];
    Real x2sqr = a.axis[2] * a.axis[2];
    Real x0x1m = a.axis[0] * a.axis[1] * oneMinusCos;
    Real x0x2m = a.axis[0] * a.axis[2] * oneMinusCos;
    Real x1x2m = a.axis[1] * a.axis[2] * oneMinusCos;
    Real x0Sin = a.axis[0] * sn;
    Real x1Sin = a.axis[1] * sn;
    Real x2Sin = a.axis[2] * sn;

#if defined(GTE_USE_MAT_VEC)
    r(0, 0) = x0sqr * oneMinusCos + cs;
    r(0, 1) = x0x1m - x2Sin;
    r(0, 2) = x0x2m + x1Sin;
    r(1, 0) = x0x1m + x2Sin;
    r(1, 1) = x1sqr * oneMinusCos + cs;
    r(1, 2) = x1x2m - x0Sin;
    r(2, 0) = x0x2m - x1Sin;
    r(2, 1) = x1x2m + x0Sin;
    r(2, 2) = x2sqr * oneMinusCos + cs;
#else
    r(0, 0) = x0sqr * oneMinusCos + cs;
    r(1, 0) = x0x1m - x2Sin;
    r(2, 0) = x0x2m + x1Sin;
    r(0, 1) = x0x1m + x2Sin;
    r(1, 1) = x1sqr * oneMinusCos + cs;
    r(2, 1) = x1x2m - x0Sin;
    r(0, 2) = x0x2m - x1Sin;
    r(1, 2) = x1x2m + x0Sin;
    r(2, 2) = x2sqr * oneMinusCos + cs;
#endif
  }

  // Convert a rotation matrix to Euler angles.  Factorization into
  // Euler angles is not necessarily unique.  If the result is
  // ER_NOT_UNIQUE_SUM, then the multiple solutions occur because
  // angleN2+angleN0 is constant.  If the result is ER_NOT_UNIQUE_DIF,
  // then the multiple solutions occur because angleN2-angleN0 is
  // constant.  In either type of nonuniqueness, the function returns
  // angleN0=0.
  static void Convert(Matrix<N, N, Real> const &r, EulerAngles<Real> &e) {
    static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");

    if (0 <= e.axis[0] && e.axis[0] < 3 && 0 <= e.axis[1] && e.axis[1] < 3 &&
        0 <= e.axis[2] && e.axis[2] < 3 && e.axis[1] != e.axis[0] &&
        e.axis[1] != e.axis[2]) {
      if (e.axis[0] != e.axis[2]) {
#if defined(GTE_USE_MAT_VEC)
        // Map (0,1,2), (1,2,0), and (2,0,1) to +1.
        // Map (0,2,1), (2,1,0), and (1,0,2) to -1.
        int parity = (((e.axis[2] | (e.axis[1] << 2)) >> e.axis[0]) & 1);
        Real const sgn = (parity & 1 ? (Real)-1 : (Real) + 1);

        if (r(e.axis[2], e.axis[0]) < (Real)1) {
          if (r(e.axis[2], e.axis[0]) > (Real)-1) {
            e.angle[2] = std::atan2(sgn * r(e.axis[1], e.axis[0]),
                                    r(e.axis[0], e.axis[0]));
            e.angle[1] = std::asin(-sgn * r(e.axis[2], e.axis[0]));
            e.angle[0] = std::atan2(sgn * r(e.axis[2], e.axis[1]),
                                    r(e.axis[2], e.axis[2]));
            e.result = ER_UNIQUE;
          } else {
            e.angle[2] = (Real)0;
            e.angle[1] = sgn * (Real)GTE_C_HALF_PI;
            e.angle[0] = std::atan2(-sgn * r(e.axis[1], e.axis[2]),
                                    r(e.axis[1], e.axis[1]));
            e.result = ER_NOT_UNIQUE_DIF;
          }
        } else {
          e.angle[2] = (Real)0;
          e.angle[1] = -sgn * (Real)GTE_C_HALF_PI;
          e.angle[0] = std::atan2(-sgn * r(e.axis[1], e.axis[2]),
                                  r(e.axis[1], e.axis[1]));
          e.result = ER_NOT_UNIQUE_SUM;
        }
#else
        // Map (0,1,2), (1,2,0), and (2,0,1) to +1.
        // Map (0,2,1), (2,1,0), and (1,0,2) to -1.
        int parity = (((e.axis[0] | (e.axis[1] << 2)) >> e.axis[2]) & 1);
        Real const sgn = (parity & 1 ? (Real) + 1 : (Real)-1);

        if (r(e.axis[0], e.axis[2]) < (Real)1) {
          if (r(e.axis[0], e.axis[2]) > (Real)-1) {
            e.angle[0] = std::atan2(sgn * r(e.axis[1], e.axis[2]),
                                    r(e.axis[2], e.axis[2]));
            e.angle[1] = std::asin(-sgn * r(e.axis[0], e.axis[2]));
            e.angle[2] = std::atan2(sgn * r(e.axis[0], e.axis[1]),
                                    r(e.axis[0], e.axis[0]));
            e.result = ER_UNIQUE;
          } else {
            e.angle[0] = (Real)0;
            e.angle[1] = sgn * (Real)GTE_C_HALF_PI;
            e.angle[2] = std::atan2(-sgn * r(e.axis[1], e.axis[0]),
                                    r(e.axis[1], e.axis[1]));
            e.result = ER_NOT_UNIQUE_DIF;
          }
        } else {
          e.angle[0] = (Real)0;
          e.angle[1] = -sgn * (Real)GTE_C_HALF_PI;
          e.angle[2] = std::atan2(-sgn * r(e.axis[1], e.axis[0]),
                                  r(e.axis[1], e.axis[1]));
          e.result = ER_NOT_UNIQUE_SUM;
        }
#endif
      } else {
#if defined(GTE_USE_MAT_VEC)
        // Map (0,2,0), (1,0,1), and (2,1,2) to +1.
        // Map (0,1,0), (1,2,1), and (2,0,2) to -1.
        int b0 = 3 - e.axis[1] - e.axis[2];
        int parity = (((b0 | (e.axis[1] << 2)) >> e.axis[2]) & 1);
        Real const sgn = (parity & 1 ? (Real) + 1 : (Real)-1);

        if (r(e.axis[2], e.axis[2]) < (Real)1) {
          if (r(e.axis[2], e.axis[2]) > (Real)-1) {
            e.angle[2] =
                std::atan2(r(e.axis[1], e.axis[2]), sgn * r(b0, e.axis[2]));
            e.angle[1] = std::acos(r(e.axis[2], e.axis[2]));
            e.angle[0] =
                std::atan2(r(e.axis[2], e.axis[1]), -sgn * r(e.axis[2], b0));
            e.result = ER_UNIQUE;
          } else {
            e.angle[2] = (Real)0;
            e.angle[1] = (Real)GTE_C_PI;
            e.angle[0] =
                std::atan2(sgn * r(e.axis[1], b0), r(e.axis[1], e.axis[1]));
            e.result = ER_NOT_UNIQUE_DIF;
          }
        } else {
          e.angle[2] = (Real)0;
          e.angle[1] = (Real)0;
          e.angle[0] =
              std::atan2(sgn * r(e.axis[1], b0), r(e.axis[1], e.axis[1]));
          e.result = ER_NOT_UNIQUE_SUM;
        }
#else
        // Map (0,2,0), (1,0,1), and (2,1,2) to -1.
        // Map (0,1,0), (1,2,1), and (2,0,2) to +1.
        int b2 = 3 - e.axis[0] - e.axis[1];
        int parity = (((b2 | (e.axis[1] << 2)) >> e.axis[0]) & 1);
        Real const sgn = (parity & 1 ? (Real)-1 : (Real) + 1);

        if (r(e.axis[0], e.axis[0]) < (Real)1) {
          if (r(e.axis[0], e.axis[0]) > (Real)-1) {
            e.angle[0] =
                std::atan2(r(e.axis[1], e.axis[0]), sgn * r(b2, e.axis[0]));
            e.angle[1] = std::acos(r(e.axis[0], e.axis[0]));
            e.angle[2] =
                std::atan2(r(e.axis[0], e.axis[1]), -sgn * r(e.axis[0], b2));
            e.result = ER_UNIQUE;
          } else {
            e.angle[0] = (Real)0;
            e.angle[1] = (Real)GTE_C_PI;
            e.angle[2] =
                std::atan2(sgn * r(e.axis[1], b2), r(e.axis[1], e.axis[1]));
            e.result = ER_NOT_UNIQUE_DIF;
          }
        } else {
          e.angle[0] = (Real)0;
          e.angle[1] = (Real)0;
          e.angle[2] =
              std::atan2(sgn * r(e.axis[1], b2), r(e.axis[1], e.axis[1]));
          e.result = ER_NOT_UNIQUE_SUM;
        }
#endif
      }
    } else {
      // Invalid angles.
      e.angle[0] = (Real)0;
      e.angle[1] = (Real)0;
      e.angle[2] = (Real)0;
      e.result = ER_INVALID;
    }
  }

  // Convert Euler angles to a rotation matrix.  The three integer
  // inputs are in {0,1,2} and correspond to world directions
  // unit(0) = (1,0,0), unit(1) = (0,1,0), or unit(2) = (0,0,1).  The
  // triples (N0,N1,N2) must be in the following set,
  //   {(0,1,2),(0,2,1),(1,0,2),(1,2,0),(2,0,1),(2,1,0),
  //    (0,1,0),(0,2,0),(1,0,1),(1,2,1),(2,0,2),(2,1,2)}
  // The rotation matrix is
  //   [GTE_USE_MAT_VEC]
  //     R(unit(N2),angleN2)*R(unit(N1),angleN1)*R(unit(N0),angleN0)
  // or
  //   [GTE_USE_VEC_MAT]
  //     R(unit(N0),angleN0)*R(unit(N1),angleN1)*R(unit(N2),angleN2)
  // The conventions of constructor Matrix3(axis,angle) apply here as
  // well.
  //
  // NOTE:  The reversal of order is chosen so that a rotation matrix
  // built with one multiplication convention is the transpose of the
  // rotation matrix built with the other multiplication convention.
  // Thus,
  // [GTE_USE_MAT_VEC]
  //   Matrix3x3<Real> R_mvconvention(N0,N1,N2,angleN0,angleN1,angleN2);
  //   Vector3<Real> V(...);
  //   Vector3<Real> U = R_mvconvention*V;  // (u0,u1,u2) = R2*R1*R0*V
  // [GTE_USE_VEC_MAT]
  //   Matrix3x3<Real> R_vmconvention(N0,N1,N2,angleN0,angleN1,angleN2);
  //   Vector3<Real> V(...);
  //   Vector3<Real> U = R_mvconvention*V;  // (u0,u1,u2) = V*R0*R1*R2
  // In either convention, you get the same 3-tuple U.
  static void Convert(EulerAngles<Real> const &e, Matrix<N, N, Real> &r) {
    static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");

    if (0 <= e.axis[0] && e.axis[0] < 3 && 0 <= e.axis[1] && e.axis[1] < 3 &&
        0 <= e.axis[2] && e.axis[2] < 3 && e.axis[1] != e.axis[0] &&
        e.axis[1] != e.axis[2]) {
      Matrix<N, N, Real> r0, r1, r2;
      Convert(AxisAngle<N, Real>(Vector<N, Real>::Unit(e.axis[0]), e.angle[0]),
              r0);
      Convert(AxisAngle<N, Real>(Vector<N, Real>::Unit(e.axis[1]), e.angle[1]),
              r1);
      Convert(AxisAngle<N, Real>(Vector<N, Real>::Unit(e.axis[2]), e.angle[2]),
              r2);
#if defined(GTE_USE_MAT_VEC)
      r = r2 * r1 * r0;
#else
      r = r0 * r1 * r2;
#endif
    } else {
      // Invalid angles.
      r.MakeIdentity();
    }
  }

  // Convert a quaternion to an axis-angle pair, where
  //   q = sin(angle/2)*(axis[0]*i+axis[1]*j+axis[2]*k)+cos(angle/2)
  static void Convert(Quaternion<Real> const &q, AxisAngle<N, Real> &a) {
    static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");

    a.axis.MakeZero();

    Real axisSqrLen = q[0] * q[0] + q[1] * q[1] + q[2] * q[2];
    if (axisSqrLen > (Real)0) {
#if defined(GTE_USE_MAT_VEC)
      Real adjust = ((Real)1) / std::sqrt(axisSqrLen);
#else
      Real adjust = ((Real)-1) / std::sqrt(axisSqrLen);
#endif
      a.axis[0] = q[0] * adjust;
      a.axis[1] = q[1] * adjust;
      a.axis[2] = q[2] * adjust;
      Real cs = std::max(std::min(q[3], (Real)1), (Real)-1);
      a.angle = (Real)2 * std::acos(cs);
    } else {
      // The angle is 0 (modulo 2*pi). Any axis will work, so choose
      // the Unit(0) axis.
      a.axis[0] = (Real)1;
      a.angle = (Real)0;
    }
  }

  // Convert an axis-angle pair to a quaternion, where
  //   q = sin(angle/2)*(axis[0]*i+axis[1]*j+axis[2]*k)+cos(angle/2)
  static void Convert(AxisAngle<N, Real> const &a, Quaternion<Real> &q) {
    static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");

#if defined(GTE_USE_MAT_VEC)
    Real halfAngle = (Real)0.5 * a.angle;
#else
    Real halfAngle = (Real)-0.5 * a.angle;
#endif
    Real sn = std::sin(halfAngle);
    q[0] = sn * a.axis[0];
    q[1] = sn * a.axis[1];
    q[2] = sn * a.axis[2];
    q[3] = std::cos(halfAngle);
  }

  // Convert a quaternion to Euler angles.  The quaternion is converted
  // to a matrix which is then converted to Euler angles.
  static void Convert(Quaternion<Real> const &q, EulerAngles<Real> &e) {
    static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");

    Matrix<N, N, Real> r;
    Convert(q, r);
    Convert(r, e);
  }

  // Convert Euler angles to a quaternion.  The Euler angles are
  // converted to a matrix which is then converted to a quaternion.
  static void Convert(EulerAngles<Real> const &e, Quaternion<Real> &q) {
    static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");

    Matrix<N, N, Real> r;
    Convert(e, r);
    Convert(r, q);
  }

  // Convert an axis-angle pair to Euler angles.  The axis-angle pair
  // is converted to a quaternion which is then converted to Euler
  // angles.
  static void Convert(AxisAngle<N, Real> const &a, EulerAngles<Real> &e) {
    static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");

    Quaternion<Real> q;
    Convert(a, q);
    Convert(q, e);
  }

  // Convert Euler angles to an axis-angle pair.  The Euler angles are
  // converted to a quaternion which is then converted to an axis-angle
  // pair.
  static void Convert(EulerAngles<Real> const &e, AxisAngle<N, Real> &a) {
    static_assert(N == 3 || N == 4, "Dimension must be 3 or 4.");

    Quaternion<Real> q;
    Convert(e, q);
    Convert(q, a);
  }
};
} // namespace gte